#!/usr/bin/env python
# -*- coding: UTF-8 -*-

import RPi.GPIO as GPIO
import time
import atexit


class Controller:
    def __init__(self):
        atexit.register(GPIO.cleanup)
        self.pitch_pin = 17  # 俯仰角控制
        self.yaw_pin = 27  # 航向角控制

        GPIO.setwarnings(False)
        GPIO.setmode(GPIO.BCM)
        GPIO.setup(self.pitch_pin, GPIO.OUT, initial=False)
        self.pitch_pwm = GPIO.PWM(self.pitch_pin, 50)  # 100HZ
        self.pitch_pwm.start(0)
        self.pitch_pwm.ChangeDutyCycle(7.5)
        GPIO.setup(self.yaw_pin, GPIO.OUT, initial=False)
        self.yaw_pwm = GPIO.PWM(self.yaw_pin, 50)  # 100HZ
        self.yaw_pwm.start(0)
        self.yaw_pwm.ChangeDutyCycle(7.5)
        time.sleep(2)

    def control_pitch(self, pitch_angle):
        if pitch_angle>120:
            pitch_angle=120
        elif pitch_angle<1:
            pitch_angle=1
        dutyCycle = 2.5 + 10.* pitch_angle / 180.
        print dutyCycle
        self.pitch_pwm.ChangeDutyCycle(dutyCycle)
        time.sleep(0.02)

    def control_yaw(self, yaw_angle):
        if yaw_angle>165:
            yaw_angle=165
        elif yaw_angle<15:
            yaw_angle=15
        dutyCycle = 2.5 + 10. * yaw_angle / 180.
        print dutyCycle
        self.yaw_pwm.ChangeDutyCycle(dutyCycle)
        time.sleep(0.02)



if __name__ == '__main__':
    mController_test = Controller()
    while True:
        ch = raw_input("请选择控制pitch(p)或yaw(y):")
        if ch == 'p':
            pitch_angle_test = input("俯仰角（1-125）：")
            mController_test.control_pitch(pitch_angle_test)
        elif ch == 'y':
            yaw_angle_test = input("航向角（30-160）：")
            mController_test.control_yaw(yaw_angle_test)
